function [u u_moved] = ex31_SolveSystem(dp_lme,s_near,x_nodes,x_samples,w_samples,parameters,options)
% function [u] =
% ex31_SolveSystem(dp_lme,s_near,x_nodes,x_samples,w_samples,parameters,options)
%
% This function assembles and and solves the displacement Patch Test.
%
% Input:
%    p_lme     : shape functions
%    s_lme     : shape functions gradient
%    s_near   : list of neighbors
%    x_nodes   : node points
%    x_samples : sample point
%    w_samples : gauss weigth for each sample point
%    parameters: L (length), D (diameter), nu (Poisson coefficient), E
%                (Young modulus)
%    options   : lme options
%
% Output:
%    u     : vectorial displacement field
%

ind_Dirichlet = parameters.ind_Dirichlet;
A             = parameters.linear_transform;

% Material parameters
E  = parameters.E;
nu = parameters.nu;

nPts = size(x_nodes,1);
sPts = size(x_samples,1);

%% ------------------------------------------------------------------------
%   Displacement imposition
u_moved = zeros(2*nPts,1);
for i = 1:nPts
    u_moved((i-1)*2+1:i*2) = A*x_nodes(i,:)' - x_nodes(i,:)';   %   u_x
end
%% ------------------------------------------------------------------------
% The right hand side rhs is computed
rhs = zeros(2*nPts,1);

%% ------------------------------------------------------------------------
%  The stiffness matrix is assembled 
nn=0;
for k=1:sPts
  nn = max(nn, length(s_near{k}));
end
nn = min(nn,nPts);

%K = spalloc(2*nPts,2*nPts,2*nn*nPts);
K = zeros(2*nPts,2*nPts);

C_stiff=E/(1+nu)/(1-2*nu)*[  1-nu,   nu,          0 ;...
	                           nu, 1-nu,          0 ;...
	                            0,    0, (1-2*nu)/2];


for ig=1:sPts

  nact=length(s_near{ig});
  B_ig=zeros(3,2*nact);
  B_ig(1,1:2:2*nact)=dp_lme{ig}(:,1)';
  B_ig(2,2:2:2*nact)=dp_lme{ig}(:,2)';
  B_ig(3,2:2:2*nact)=dp_lme{ig}(:,1)';
  B_ig(3,1:2:2*nact)=dp_lme{ig}(:,2)';
  K_ig_loc=B_ig'*C_stiff*B_ig;

  %assembly
  active=s_near{ig};
  K(2*active(:)-1,2*active(:)-1) = ...
      K(2*active(:)-1,2*active(:)-1) + ...
      K_ig_loc(1:2:2*nact,1:2:2*nact)*w_samples(ig);
  K(2*active(:),2*active(:)-1) = ...
      K(2*active(:),2*active(:)-1) + ...
      K_ig_loc(2:2:2*nact,1:2:2*nact)*w_samples(ig);
  K(2*active(:)-1,2*active(:)) = ...
      K(2*active(:)-1,2*active(:)) + ...
      K_ig_loc(1:2:2*nact,2:2:2*nact)*w_samples(ig);
  K(2*active(:),2*active(:)) = ...
      K(2*active(:),2*active(:)) + ...
      K_ig_loc(2:2:2*nact,2:2:2*nact)*w_samples(ig);
end



%% Dirichlet BCs are applied

%(x=0,y=0)
ind                = ind_Dirichlet;
K(2*ind,:)         = 0;% <----FEM Imposition
%K(:,2*ind)         = 0;% <----FEM Imposition
K(2*ind-1,:)       = 0;% <----FEM Imposition
%K(:,2*ind-1)       = 0;% <----FEM Imposition
rhs(2*ind)         = u_moved(2*ind);% <---- Traslated node
rhs(2*ind-1)       = u_moved(2*ind-1);% <---- Traslated node
%rhs         = u_moved;% <---- Traslated node
for ind = ind_Dirichlet
    K(2*ind,2*ind)     = 1;% <----FEM Imposition
    K(2*ind-1,2*ind-1) = 1;% <----FEM Imposition
end
%% ------------------------------------------------------------------------
% The system is solved
u=K\rhs;

